#include "MyAsp.h"
#include "MotAsp_Mgr.c"
#include "MotAsp_Cfg.c"
#include "MotAsp_DiagCbk.c"
#include "MotAsp_Lib.c"

void MyAsp_1ms(PieFbAspBus *const PieFb,
    CtrlToAspBus *const CtrlCmd,
    AspFbCtrlBus *AspFbCtrl)
{
    AspFbCtrl->Pos_0p01Rad=PieFb->CosP+CtrlCmd->ReqSta;
    MotAsp_Mgr_Spd_Pos_Handler(&MotAsp);
}

void MyAsp_100us(
    PieFbAspBus *const PieFb,
    AspFbDrvBus *AspFbDrv)
{ 
    // MotAsp.Pri.MPS.D_SiCo_i[MotAsp_Lib_SIN_0].SinCos_V = PieFb->SinN;
    // MotAsp.Pri.MPS.D_SiCo_i[MotAsp_Lib_SIN_1].SinCos_V = PieFb->SinP;
    // MotAsp.Pri.MPS.D_SiCo_i[MotAsp_Lib_COS_0].SinCos_V = PieFb->CosN;
    // MotAsp.Pri.MPS.D_SiCo_i[MotAsp_Lib_COS_1].SinCos_V = PieFb->CosP;

    // if((MotAsp_Base_STrig_HigLev == MotAsp->In.Ctrl.CalOffsetAngle_RiseTrig)&&(MotAsp_Base_CAStep_Agl_0 == MotAsp->In.Ctrl.CAStep))
    // {
    //      MotAsp->Pri.Learn_E.En = MotAsp_Base_Valid;
    // }
    // else
    // {
    //      MotAsp->Pri.Learn_E.En = MotAsp_Base_InValid;
    // }

    // if(MotAsp_Base_STrig_HigLev == MotAsp->In.Ctrl.CalOffsetPos_RiseTrig)
    // {
    //      MotAsp->Pri.Learn_P.En = MotAsp_Base_Valid;
    // }
    // else
    // {
    //      MotAsp->Pri.Learn_P.En = MotAsp_Base_InValid;
    // }

    // if(MotAsp_Base_SMode_OpenLoop == MotAsp->In.Ctrl.Mode)     
    // {
    //      MotAsp->Pri.Open.Rpm = MotAsp->In.Ctrl.VirSpdRpm;
    // }
    // else
    // {
    //      MotAsp->Pri.Open.Rpm =  0;
    // }

    MotAsp_Mgr_Angle_Handler(&MotAsp);

    // MotAsp->Out.Ctrl.PosRel_01mm            =              MotAsp->Pri.P_Rel.Result;
    // MotAsp->Out.Ctrl.PosRel_Sta             =              MotAsp->Pri.Learn_E.Result.Sta;

    // MotAsp->Out.Ctrl.PosAbs_01mm            =              MotAsp->Pri.P_Abs.Result;
    // MotAsp->Out.Ctrl.PosAbs_Sta             =              MotAsp->Pri.Learn_P.Result.Sta;

    // MotAsp->Out.Ctrl.SpdRpm                 =              MotAsp->Pri.Spd_Calc.Spd;

    // if(MotAsp_Base_SMode_OpenLoop == MotAsp->In.Ctrl.Mode)
    // {
    //      MotAsp->Out.Drv.eAngle             =              MotAsp->Pri.Open.Angle;
    // }
    // else
    // {
    //      MotAsp->Out.Drv.eAngle             =              MotAsp->Pri.MPS.Arctan.Angle - MotAsp->Pri.Learn_E.Result.Val;
    //      MotAsp->Out.Drv.eAngleSta          =              MotAsp->Pri.Learn_E.Result.Sta;

    //      MotAsp->Out.Ctrl.mAngle            =              MotAsp->Pri.MPS.Sp.M_Angle - MotAsp->Pri.Learn_E.M_Result.Val;
    //      MotAsp->Out.Ctrl.mAngleSta         =              MotAsp->Pri.Learn_E.M_Result.Sta;
    // }
}